#include "Motor.h"
    Motor::Motor() {
        stop();
    }

    void Motor::stop()
    {
        send(192);
        lastRightSpeed = 0;
        lastLeftSpeed = 0;
    }
    void Motor::go()
    {
        if (!reportSpeed) {
            send(193);
        }
        else {
            send(225);
            lastRightSpeed = send(REPORT_SPEED);
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::reverse()
    {
        if (!reportSpeed) {
            send(194);
        }
        else {
            send(226);
            lastRightSpeed = send(REPORT_SPEED);
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::goOffset(unsigned char offset)
    {
        if (!reportSpeed) {
            send(195);
            send(offset);
        }
        else {
            send(227);
            send(offset);
            lastRightSpeed = send(REPORT_SPEED);
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::reverseOffset(unsigned char offset)
    {
        if (!reportSpeed) {
            send(196);
            send(offset);
        }
        else {
            send(228);
            send(offset);
            lastRightSpeed = send(REPORT_SPEED);
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::spinRight()
    {
        if (!reportSpeed) {
            send(197);
        }
        else {
            send(229);
            lastRightSpeed = send(REPORT_SPEED);
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::spinLeft()
    {
        if (!reportSpeed) {
            send(198);
        }
        else {
            send(230);
            lastRightSpeed = send(REPORT_SPEED);
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::turnRight()
    {
        if (!reportSpeed) {
            send(199);
        }
        else {
            send(231);
            lastRightSpeed = 0;
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::turnLeft()
    {
        if (!reportSpeed) {
            send(200);
        }
        else {
            send(232);
            lastRightSpeed = send(REPORT_SPEED);
            lastLeftSpeed = 0;
        }
    }
    void Motor::rightWheelStop()
    {
        send(64);
        lastRightSpeed = 0;
    }
    void Motor::rightWheelGo()
    {
        if (!reportSpeed) {
            send(65);
        }
        else {
            send(97);
            lastRightSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::rightWheelReverse()
    {
        if (!reportSpeed) {
            send(66);
        }
        else {
            send(98);
            lastRightSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::rightWheelGoOffset(unsigned char offset)
    {
        if (!reportSpeed) {
            send(67);
            send(offset);
        }
        else {
            send(99);
            send(offset);
            lastRightSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::rightWheelReverseOffset(unsigned char offset)
    {
        if (!reportSpeed) {
            send(68);
            send(offset);
        }
        else {
            send(100);
            send(offset);
            lastRightSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::rightWheelVar(unsigned char speed)
    {
        if (!reportSpeed) {
            send(69);
        }
        else {
            send(101);
            send(speed);
            lastRightSpeed = speed;
        }
    }
    void Motor::leftWheelStop()
    {
        send(128);
        lastLeftSpeed = 0;
    }
    void Motor::leftWheelGo()
    {
        if (!reportSpeed) {
            send(129);
        }
        else {
            send(161);
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::leftWheelReverse()
    {
        if (!reportSpeed) {
            send(130);
        }
        else {
            send(162);
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::leftWheelGoOffset(unsigned char offset)
    {
        if (!reportSpeed) {
            send(131);
            send(offset);
        }
        else {
            send(163);
            send(offset);
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::leftWheelReverseOffset(unsigned char offset)
    {
        if (!reportSpeed) {
            send(132);
            send(offset);
        }
        else {
            send(164);
            send(offset);
            lastLeftSpeed = send(REPORT_SPEED);
        }
    }
    void Motor::leftWheelVar(unsigned char speed)
    {
        if (!reportSpeed) {
            send(133);
        }
        else {
            send(165);
            send(speed);
            lastLeftSpeed = speed;
        }
    }

    void Motor::setReportSpeed(bool rs) {
        reportSpeed = rs;
    }

    unsigned char Motor::getLastRightSpeed() {
        return lastRightSpeed;
    }
    unsigned char Motor::getLastLeftSpeed() {
        return lastLeftSpeed;
    }
